/*
 * <one line to give the program's name and a brief idea of what it does.>
 * Copyright (C) 2016  <copyright holder> <email>
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include "pointcloudmapping.h"
#include <KeyFrame.h>
#include <opencv2/highgui/highgui.hpp>
#include <pcl/filters/passthrough.h>
#include <pcl/common/transforms.h>
#include <pcl/io/pcd_io.h>
#include "Converter.h"
#include "Eigen/Dense"
#include "Eigen/Geometry"

//pcl::visualization::CloudViewer PointCloudMapping::mViewer("Viewer");

PointCloudMapping::PointCloudMapping(double resolution_):
        mbStopped(false), mbStopRequested(false), mbShutDownRequested(false), mShutDownFlag(true)
{
    this->resolution = resolution_;
    voxel.setLeafSize( resolution, resolution, resolution);

    this->sor.setMeanK(50);                                //设置在进行统计时考虑查询点邻近点数
    this->sor.setStddevMulThresh(1.0);                    //设置判断是否为离群点的阈值

    mpGlobalMap = boost::make_shared< PointCloud >( );

    //mViewerThread = make_shared<thread>( bind(&PointCloudMapping::Run, this ) );

}

void PointCloudMapping::InsertKeyFrame(KeyFrame* pkf)
{
    cout<<"receive a keyframe, id = "<<pkf->mnId<<endl;
    unique_lock<mutex> lck(mKeyFrameMutex);
    mKeyFrames.push_back( pkf );
    mColorImgs.push_back( pkf->iColorImg.clone() );
    mDepthImgs.push_back( pkf->iDepthImg.clone() );
    mNewKeyFrame.push_back(mKeyFrames.size()-1);
}

pcl::PointCloud< PointCloudMapping::PointT >::Ptr PointCloudMapping::generatePointCloud(KeyFrame* kf, cv::Mat& color, cv::Mat& depth)
{
    PointCloud::Ptr tmp( new PointCloud() );
    // point cloud is null ptr
    for ( int m=0; m<depth.rows; m+=3 )
    {
        for ( int n=0; n<depth.cols; n+=3 )
        {
            float d = depth.ptr<float>(m)[n];
            if (d < 0.01 || d>10)
                continue;
            PointT p;
            p.z = d;
            p.x = ( n - kf->cx) * p.z / kf->fx;
            p.y = ( m - kf->cy) * p.z / kf->fy;

            p.b = color.ptr<uchar>(m)[n*3];
            p.g = color.ptr<uchar>(m)[n*3+1];
            p.r = color.ptr<uchar>(m)[n*3+2];
            tmp->points.push_back(p);
        }
    }

    Eigen::Isometry3d T = ORB_SLAM2::Converter::toSE3Quat( kf->GetPose() );
    PointCloud::Ptr cloud(new PointCloud);
    pcl::transformPointCloud( *tmp, *cloud, T.inverse().matrix());
    cloud->is_dense = false;

    cout<<"generate point cloud for kf "<<kf->mnId<<", size="<<cloud->points.size()<<endl;
    return cloud;
}

void PointCloudMapping::Run()
{
    mShutDownFlag = false;

    while(1)
    {
        if(CheckNewKeyFrame())
        {
            JointNewKeyFrame();
            UpdateShow();
        }

        if(Stop())
        {
            while(isStopped())
            {
                usleep(3000);
            }
        }

        if(CheckShutDown())
        {
            break;
        }
    }

    SetShutDown();

    //Save();
}

void PointCloudMapping::public_cloud(pcl::PointCloud< pcl::PointXYZRGBA >& cloud_out)
{
    unique_lock<mutex> lockMap(mMutexGlobalMap);
    cloud_out = mPointCloud;
}

void PointCloudMapping::Cloud_transform(pcl::PointCloud< PointCloudMapping::PointT >::Ptr& source, pcl::PointCloud< PointCloudMapping::PointT >::Ptr& out)
{
    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    PointCloud::Ptr cloud_filtered(new PointCloud());
    transform.translation() << 0.0, 0.0, 0.35;
    transform.rotate (Eigen::AngleAxisf (1.5*M_PI, Eigen::Vector3f::UnitX()));
    pcl::transformPointCloud (*source, *cloud_filtered, transform);

    //直通滤波器
    pass.setInputCloud (cloud_filtered);            //设置输入点云
    pass.setFilterFieldName ("z");             //设置过滤时所需要点云类型的Z字段
    pass.setFilterLimits (-1.0, 1.0);
    pass.filter (*out);
}

bool PointCloudMapping::CheckNewKeyFrame()
{
    unique_lock<mutex> lck(mKeyFrameMutex);
    return (!mNewKeyFrame.empty());
}

void PointCloudMapping::JointNewKeyFrame()
{
    KeyFrame* pCurrentKeyFrame;
    cv::Mat color, depth;
    //! get KeyFrame form list
    {
        unique_lock<mutex> lck(mKeyFrameMutex);
        uint64_t nid = mNewKeyFrame.front();
        mNewKeyFrame.pop_front();
        pCurrentKeyFrame = mKeyFrames[nid];
        color = mColorImgs[nid].clone();
        depth = mDepthImgs[nid].clone();
    }

    {
        unique_lock<mutex> lockMap(mMutexGlobalMap);
        PointCloud::Ptr p = generatePointCloud(pCurrentKeyFrame, color, depth);
        *mpGlobalMap += *p;

        PointCloud::Ptr pTempCloud(new PointCloud());
        voxel.setInputCloud(mpGlobalMap);
        voxel.filter(*pTempCloud);
        mpGlobalMap->swap(*pTempCloud);
    }
}

void PointCloudMapping::UpdateShow()
{
    unique_lock<mutex> lockMap(mMutexGlobalMap);

    mPointCloud = *mpGlobalMap;
    mpGlobalMap = mPointCloud.makeShared();

    //mViewer.showCloud(mpGlobalMap);                                        // show the pointcloud without optimization
    cout << "show global map, size=" << mpGlobalMap->points.size() << endl;
}

void PointCloudMapping::Save()
{
    unique_lock<mutex> lockMap(mMutexGlobalMap);
    mpGlobalMap->clear();
    for(size_t i=0;i<mKeyFrames.size();i++)                               // save the optimized pointcloud
    {
        cout<<"keyframe "<<i<<" ..."<<endl;
        PointCloud::Ptr tp = generatePointCloud(mKeyFrames[i], mColorImgs[i], mDepthImgs[i]);
        PointCloud::Ptr tmp(new PointCloud());
        voxel.setInputCloud( tp );
        voxel.filter( *tmp );
        *mpGlobalMap += *tmp;
        //mViewer.showCloud(mpGlobalMap);
    }
    PointCloud::Ptr pTempCloud(new PointCloud());
    sor.setInputCloud(mpGlobalMap);
    sor.filter(*pTempCloud);
    mpGlobalMap->swap(*pTempCloud);
    pcl::io::savePCDFileBinary ( "optimized_pointcloud.pcd", *mpGlobalMap );
    cout<<"Save point cloud file successfully!"<<endl;
}

void PointCloudMapping::RequestStop()
{
    unique_lock<mutex> lock(mMutexStop);
    if(!mbStopped)
        mbStopRequested = true;
}

void PointCloudMapping::RequestStart()
{
    unique_lock<mutex> lock(mMutexStop);
    if(mbStopped)
    {
        mbStopRequested = false;
        mbStopped = false;
    }
}

bool PointCloudMapping::isStopped()
{
    unique_lock<mutex> lock(mMutexStop);
    return mbStopped;
}

bool PointCloudMapping::Stop()
{
    unique_lock<mutex> lock(mMutexStop);

    if(mbShutDownRequested)
        return false;
    else if(mbStopRequested)
    {
        mbStopped = true;
        mbStopRequested = false;
        return true;
    }

    return false;
}

void PointCloudMapping::Release()
{
    unique_lock<mutex> lock(mMutexStop);
    unique_lock<mutex> lockMap(mMutexGlobalMap);
    mbStopped = false;

    mKeyFrames.clear();
    mColorImgs.clear();
    mDepthImgs.clear();
    mNewKeyFrame.clear();

    mpGlobalMap->clear();
}

void PointCloudMapping::RequestShutDown()
{
    unique_lock<mutex> lck(mMutexShutDown);
    mbShutDownRequested = true;
}

bool PointCloudMapping::CheckShutDown()
{
    unique_lock<mutex> lck(mMutexShutDown);
    return mbShutDownRequested;
}

void PointCloudMapping::SetShutDown()
{
    {
        unique_lock<mutex> lck(mMutexShutDown);
        mShutDownFlag = true;
    }
    //mViewerThread->join();
}